#pragma once
#include <string>
#include <stack>
#include <memory>
#include <functional>
#include <shared_mutex>
#include "axis/planner.h"
#include "axis/backend_commu.h"

namespace rtbus {
class RtCommuItf;
}  // namespace rtbus
namespace axis {

const size_t MAX_SLAVE_SERVO_CNT = 15;

const uint16_t SDO_INDEX_ROB_DH_ENABLE = 1005;
const uint16_t SDO_INDEX_ROB_DH_VERSION = 9201;
const uint16_t SDO_INDEX_ROB_DH_CRC = 9200;
const uint16_t SDO_INDEX_ROB_DH_A = 9102;
const uint16_t SDO_INDEX_ROB_DH_ALPHA = 9103;
const uint16_t SDO_INDEX_ROB_DH_D = 9104;
const uint16_t SDO_INDEX_ROB_DH_THETA = 9105;
const uint16_t SDO_INDEX_SERVO_VERSION = 9007;
const uint16_t SDO_INDEX_SERVO_FIRST_POWERON_DATE = 9013;
const uint16_t SDO_INDEX_SERVO_HOME_VAL = 2215;         //set
const uint16_t SDO_INDEX_SERVO_MIN_POS = 2233;          //set
const uint16_t SDO_INDEX_SERVO_MAX_POS = 2232;          //set
const uint16_t SDO_INDEX_SERVO_BRAKE = 2240;            //set
const uint16_t SDO_INDEX_SERVO_MULTI_TURN_STEP = 2241;  //set
const uint16_t SDO_INDEX_SERVO_TORQ_UNIT = 9901;        //set
const uint16_t SDO_INDEX_SERVO_COLLISION_BASE = 2245;
const uint16_t SDO_INDEX_SERVO_COLLISION_TORQ = 2246;
const uint16_t SDO_INDEX_ROB_SERIALNUM = 9009;
const uint16_t SDO_INDEX_SERVO_MOTOR_SUPPLIER = 2000;
const uint16_t SDO_INDEX_SERVO_TEMPERATURE = 9108;
const uint16_t SDO_INDEX_SERVO_AVERAGE_POWER = 9101;
const uint16_t SDO_INDEX_SERVO_FLUCTUATION = 9102;
const uint16_t SDO_INDEX_SERVO_TOTAL_RUN_LAPS = 9103;
const uint16_t SDO_INDEX_SERVO_TOTAL_RUN_TIME = 9104;
const uint16_t SDO_INDEX_SERVO_CUR_RUN_LAPS = 9105;
const uint16_t SDO_INDEX_SERVO_CUR_RUN_TIME = 9106;
const uint16_t SDO_INDEX_SERVO_CAB_TYPE = 9011;            //set
const uint16_t SDO_INDEX_SERVO_BACK_DRIVE_ENABLE = 1017;   //set
const uint16_t SDO_INDEX_SERVO_AVOID_POS_MUTATION = 1018;  //set

const uint8_t OPMODE_UNKNOWN = 0;
const uint8_t OPMODE_HOME = 6;
const uint8_t OPMODE_CSP = 8;
const uint8_t OPMODE_CST = 10;

class Servo
{
public:
    Servo();
    ~Servo();
    /**
     * @brief 系统启动时初始化伺服
    */
    void init(int id);

    /**
     * @brief 上电初始化伺服
    */
    void boot_init(int busid, int devid, const bool* sim = nullptr);

    /**
     * @brief 关闭电源伺服
    */
    void shutdown();

    int config_offset_dir(int32_t offset, bool positive_dir = true);

    inline void reset_error() { usr_cmd_.error_reset = true; }
    void reset_collision() { usr_cmd_.collision_reset = 3; }

    inline int8_t get_dir() const { return config_.dir_; }
    inline int32_t get_zero_offset() const { return config_.zero_offset_; }
    inline void set_collision_level(uint8_t lv) { usr_cmd_.collision_lv_ = lv; }
    inline uint8_t get_collision_level() const { return usr_cmd_.collision_lv_; }
    inline uint16_t get_pos_diff() const { return commu.rx_data_.ota_pos_diff; }
    inline uint16_t get_dual_encoder_diff() const { return commu.rx_data_.ota_dual_encoder_diff; }
    inline uint16_t get_dc_vol() const { return commu.rx_data_.ota_dc_vol; }
    inline int16_t get_torque_sensor() const { return stat_.sensor_torque; }
    inline int get_cur_pos() const { return feedback_.pos; }
    inline int get_cmd_pos() const { return cmd_.pos; }
    inline int get_cur_torque() const { return feedback_.torque; }
    void set_opmode(uint8_t mode) { usr_cmd_.opmode = mode; }
    /**
     * @brief 在Disable状态下，将当前实际位置设置为指定值。
     * @param pos 设定值。注意当前仅支持0xffffffff，用于设置为0
     * @param is_jaka 是否为JAKA伺服,JAKA伺服会将偏移值保存在伺服里，其他的需要记录在控制中。目前当前参数仅支持true
    */
    int set_position(int pos, bool is_jaka = true);
    void set_home_cmd(uint8_t cmd) { usr_cmd_.home = cmd; }
    inline int get_cur_opmode() const { return stat_.act_opmode; }
    uint32_t get_ecode(uint8_t* category = nullptr) const;
    inline void set_dyn_forward(bool en) { usr_cmd_.enable_dyn_forward = en; }
    inline bool is_dyn_forward() const { return usr_cmd_.enable_dyn_forward; }
    inline bool is_torque_over() const { return stat_.tor_over_flg == 0xf0; }
    inline bool is_backdriving() const { return stat_.backdrive_flg == 1; }
    inline bool is_sim() const { return config_.sim_flag; }
    inline bool is_homing_done() const { return stat_.home_sta == 1; }
    inline bool is_error() const { return stat_.error_sta || stat_.collision; }
    inline bool is_collision() const { return stat_.collision; }
    inline bool is_alive() const { return alive_flag_; }
    void set_enable(bool enable) { usr_cmd_.enable = enable; }
    inline bool is_enabled() const { return stat_.enable; }
    inline int get_id() const { return config_.slave_id; }
    inline bool is_active() const { return config_.actived; }

    void update();
    int set_cmd_pos(int pos);
    int set_cmd_torque(int torque, int grv_comp);

    int comm_write_control_word(uint16_t& cword);
    int comm_write_target_pos(int32_t& pos);
    int comm_write_target_torque(int16_t& torque);
    int comm_write_opmode(uint8_t& opmode);
    int comm_write_led(uint8_t& led);

    int comm_read_status_word(uint16_t status, bool has_ext_enable_bit = true);
    int comm_read_actpos(int32_t actpos);
    int comm_read_opdis(uint8_t opdis);

    int get_sdo(uint16_t obj_id, std::function<int(uint8_t*, int)> cb, int timeout = 80, int retry = 3);
    int set_sdo(uint16_t obj_id, int* val, std::function<int(uint8_t*, int)> cb, int timeout = 80, int retry = 3);

    void comm_jkcan_map();
    void comm_jkcan_tx_map(int busid);
    void comm_ecat_map();

    void keep_alive();

private:
    struct Config
    {
        uint16_t slave_id;
        int bus_id_;     //上电后第一次收到报文对应的通讯总线ID。用于后续的所有通讯
        int bus_devid_;  //通讯层的设备ID

        bool sim_flag;
        bool actived;  // 轴是否启用，取决于配置文件
        int32_t zero_offset_;
        int8_t dir_;  //1:forward, -1:reverse
    } config_;

    bool alive_flag_;     //通信在线标记
    int alive_watchdog_;  //通讯超时看门狗

    struct UsrCmd
    {
        bool enable;  //使能控制
        uint8_t collision_reset;
        bool error_reset;
        uint8_t home;  //0-无回零 1-回零指令
        uint8_t opmode;
        bool enable_dyn_forward;
        uint8_t collision_lv_;  //碰撞检测等级给定
        uint16_t control_word_;
    } usr_cmd_;

    struct StateData
    {
        bool enable;                     //使能状态
        bool collision;                  //true：发生碰撞 false：未碰撞
        uint8_t error_sta;               //0-无 1-fault
        uint8_t home_sta;                //0-未回零 1-已回零
        uint8_t collision_lv_feedback_;  //碰撞检测等级反馈
        uint8_t backdrive_flg;           //反向驱动状态标志
        uint8_t tor_over_flg;            //转矩超限
        uint8_t act_opmode;
        int16_t sensor_torque;  // 阿童木机型，使用7、8字节储存力传感器数据
        uint16_t status_word;
    } stat_;

    struct Cmd
    {
        int pos;
        int grvcomp;
        int torque;  //伺服转矩百分比
    };

    Cmd cmd_, prev_cmd_;  //当前控制周期指令当量,不经过方向和零点偏移
    Cmd cmd_servo_;       //实际给到驱动器的指令
    struct Feedback
    {
        int pos;
        int torque;  //伺服转矩百分比
    };
    Feedback feedback_;        //反馈当量,经过方向和零点偏移反向处理
    Feedback feedback_servo_;  //实际驱动器的反馈

public:
    uint32_t version_;
    uint32_t first_poweron_date_;
    int joint_min_limit_;
    int joint_max_limit_;
    int collision_base_threshold_;    //碰撞检测基础阈值
    int collision_torque_threshold_;  //碰撞检测档位阈值
    int supplier_id_;
    int temperature_;
    int average_power_;
    int current_fluctuation_;  //电流波动百分比
    int total_run_laps_;       //总计运行圈数
    int total_run_time_;       //总计运行时间
    int current_run_laps_;     //当前次上电后运行圈数
    int current_run_time_;     //当前次上电后运行时间
    int voltage_;              //母线电压

private:
    uint16_t handle_control_word(uint16_t sword, bool has_ext_enable_bit);
    void check_alive();
    void collect_data();
    void reset_data();

public:
    // OBJECT ID
    static const uint32_t SERVO_OBJID_PARA_PROTECT = 1000;
    static const uint32_t SERVO_OBJID_SERVO_VERSION = 9007;
    static const uint32_t SERVO_OBJID_POWER_ON_DATE = 9013;
    static const uint32_t SERVO_OBJID_SEROHOME = 2215;
    static const uint32_t SERVO_OBJID_CAB_TYPE = 9011;
    static const uint32_t SERVO_OBJID_JOINT_MIN_LIMIT = 2233;
    static const uint32_t SERVO_OBJID_JOINT_MAX_LIMIT = 2232;
    static const uint32_t SERVO_OBJID_JOINT_FRICTION = 2222;
    static const uint32_t SERVO_OBJID_BREAK_RELEASE = 2240;
    static const uint32_t SERVO_OBJID_MULYI_TURN_STEP = 2241;
    static const uint32_t SERVO_OBJID_TORQUE_UNIT = 9901;
    static const uint32_t SERVO_OBJID_COLLISION_BASIC = 2245;
    static const uint32_t SERVO_OBJID_COLLISION_TH = 2246;
    static const uint32_t SERVO_OBJID_SERIAL_NUM = 9009;
    static const uint32_t SERVO_OBJID_SUPPLIER_ID = 2000;
    static const uint32_t SERVO_OBJID_TEMPERATURE = 9108;
    static const uint32_t SERVO_OBJID_AVERRAGE_POWER = 9101;
    static const uint32_t SERVO_OBJID_CURRENT_FLUCTUAION = 9102;
    static const uint32_t SERVO_OBJID_SUM_RUNNING_CYCLE = 9103;
    static const uint32_t SERVO_OBJID_SUM_RUNNING_TIME = 9104;
    static const uint32_t SERVO_OBJID_CUR_RUNNING_CYCLE = 9105;
    static const uint32_t SERVO_OBJID_CUR_RUNNING_TIME = 9106;
    static const uint32_t SERVO_OBJID_VOLTAGE = 9107;
    static const uint32_t SERVO_OBJID_BACKDRIVE = 1017;
    static const uint32_t SERVO_OBJID_POSMUTATION = 1018;

public:
    std::shared_ptr<rtbus::RtCommuItf> master_;
    CommuPort commu;
};

Servo* get_servo(uint16_t id);
void init_servo();
}  // namespace axis
